#include "leg_motor_hardware_interface/leg_motor_socketcan.hpp"

#include <cmath>

namespace leg_motor_hardware_interface
{
leg_motor_socketcan::leg_motor_socketcan()
{

}
leg_motor_socketcan::~leg_motor_socketcan()
{
    leg_motor_shutdown();
}

bool leg_motor_socketcan::init(const char* ifname)
{
    // 创建socket
    if ((socket_fd_ = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
        perror("Error while opening socket");
        return false;
    }
    // 设置CAN接口名称
    std::strcpy(ifr_.ifr_name, ifname);
    if (ioctl(socket_fd_, SIOCGIFINDEX, &ifr_) < 0) {
        perror("Error getting interface index");
        close(socket_fd_);
        return false;
    }
    // 绑定socket到CAN接口
    addr_.can_family = AF_CAN;
    addr_.can_ifindex = ifr_.ifr_ifindex;
    if (bind(socket_fd_, (struct sockaddr *)&addr_, sizeof(addr_)) < 0) {
        perror("Error binding socket");
        close(socket_fd_);
        return false;
    }
    return true;
}

bool leg_motor_socketcan::leg_motor_start()
{
    struct can_frame rframe;
    struct can_frame sframe;
    sframe.can_id = leg_motor_id_;
    // 设置控制模式为力矩控制
    sframe.can_dlc = 6;
    sframe.data[0] = 0x00U;
    sframe.data[1] = 0x4EU;
    sframe.data[2] = 0x00U;
    sframe.data[3] = 0x00U;
    sframe.data[4] = 0x00U;
    sframe.data[5] = 0x01U;
    if (write(socket_fd_, &sframe, sizeof(struct can_frame)) < 0) {
        perror("Error writing to socket");
        return false;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return false;
    }
    if (rframe.data[0] != 0x3EU)
    {
        perror("设置大腿电机力矩模式异常");
        return false;
    }

    // 设置控制源为不使用
    sframe.can_dlc = 6;
    sframe.data[0] = 0x01U;
    sframe.data[1] = 0x12U;
    sframe.data[2] = 0x00U;
    sframe.data[3] = 0x00U;
    sframe.data[4] = 0x00U;
    sframe.data[5] = 0x00U;
    if (write(socket_fd_, &sframe, sizeof(struct can_frame)) < 0) {
        perror("Error writing to socket");
        return false;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return false;
    }
    if (rframe.data[0] != 0x3EU)
    {
        perror("设置大腿电机控制源异常");
        return false;
    }

    // 设置模拟量类型为内部使用
    sframe.can_dlc = 6;
    sframe.data[0] = 0x01U;
    sframe.data[1] = 0xFDU;
    sframe.data[2] = 0x00U;
    sframe.data[3] = 0x00U;
    sframe.data[4] = 0x00U;
    sframe.data[5] = 0x00U;
    if (write(socket_fd_, &sframe, sizeof(struct can_frame)) < 0) {
        perror("Error writing to socket");
        return false;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return false;
    }
    if (rframe.data[0] != 0x3EU)
    {
        perror("设置大腿电机模拟量为内部使用异常");
        return false;
    }

    // 设置模拟量初始值(设置为0)
    sframe.can_dlc = 6;
    sframe.data[0] = 0x01U;
    sframe.data[1] = 0xFEU;
    sframe.data[2] = 0x00U;
    sframe.data[3] = 0x00U;
    sframe.data[4] = 0x00U;
    sframe.data[5] = 0x00U;
    if (write(socket_fd_, &sframe, sizeof(struct can_frame)) < 0) {
        perror("Error writing to socket");
        return false;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return false;
    }
    if (rframe.data[0] != 0x3EU)
    {
        perror("设置大腿电机模拟量初始值异常");
        return false;
    }

    // 设置最大速度限制(设为250000count/s)
    sframe.can_dlc = 6;
    sframe.data[0] = 0x02U;
    sframe.data[1] = 0x04U;
    sframe.data[2] = 0x00U;
    sframe.data[3] = 0x03U;
    sframe.data[4] = 0xD0U;
    sframe.data[5] = 0x90U;
    if (write(socket_fd_, &sframe, sizeof(struct can_frame)) < 0) {
        perror("Error writing to socket");
        return false;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return false;
    }
    if (rframe.data[0] != 0x3EU)
    {
        perror("设置大腿电机最大速度异常");
        return false;
    }

    // 电机使能
    sframe.can_dlc = 6;
    sframe.data[0] = 0x01U;
    sframe.data[1] = 0x00U;
    sframe.data[2] = 0x00U;
    sframe.data[3] = 0x00U;
    sframe.data[4] = 0x00U;
    sframe.data[5] = 0x01U;
    if (write(socket_fd_, &sframe, sizeof(struct can_frame)) < 0) {
        perror("Error writing to socket");
        return false;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return false;
    }
    if (rframe.data[0] != 0x3EU)
    {
        perror("大腿电机使能异常");
        return false;
    }
    return true;
}

bool leg_motor_socketcan::leg_motor_shutdown()
{
    struct can_frame rframe;
    struct can_frame sframe;
    sframe.can_dlc = 6;
    sframe.can_id = leg_motor_id_;
    sframe.data[0] = 0x01U;
    sframe.data[1] = 0x00U;
    sframe.data[2] = 0x00U;
    sframe.data[3] = 0x00U;
    sframe.data[4] = 0x00U;
    sframe.data[5] = 0x00U;
    if (write(socket_fd_, &sframe, sizeof(struct can_frame)) < 0) {
        perror("Error writing to socket");
        return false;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return false;
    }
    if (rframe.data[0] != 0x3EU)
    {
        perror("大腿电机失能异常");
        return false;
    }
    close(socket_fd_);
    return true;
}

void leg_motor_socketcan::leg_motor_effort_control(double effort)
{
    struct can_frame rframe;
    struct can_frame sframe;
    sframe.can_dlc = 6;
    sframe.can_id = leg_motor_id_;
    // 将目标扭矩转换为目标电流(mA)
    double iq;
    if (effort > 0.0)
    {
         iq = (129.7945*effort + 53.3847);
    }
    else if (effort < 0.0)
    {
         iq = (129.7945*effort - 53.3847);
    }
    else
    {
         iq = 0.0;
    }
    sframe.data[0] = 0x01U;
    sframe.data[1] = 0xFEU;
    sframe.data[2] = (uint8_t)((int32_t)iq >> 24 & 0xFFU);
    sframe.data[3] = (uint8_t)((int32_t)iq >> 16 & 0xFFU);
    sframe.data[4] = (uint8_t)((int32_t)iq >>  8 & 0xFFU);
    sframe.data[5] = (uint8_t)((int32_t)iq & 0xFFU);
    if (write(socket_fd_, &sframe, sizeof(struct can_frame)) < 0) {
        perror("Error writing to socket");
        return;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return;
    }
}

double leg_motor_socketcan::leg_motor_position_read()
{
    struct can_frame rframe;
    struct can_frame sframe;
    sframe.can_dlc = 2;
    sframe.can_id = leg_motor_id_;
    sframe.data[0] = 0x00U;
    sframe.data[1] = 0x02U;
    if (write(socket_fd_, &sframe, sizeof(struct can_frame)) < 0) {
        perror("Error writing to socket");
        return NAN;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return NAN;
    }
    // 返回的位置单位为count,转换为rad
    if (rframe.can_dlc != 1)
    {
        int32_t count = ((int32_t)(rframe.data[0]) << 24)
                      + ((int32_t)(rframe.data[1]) << 16)
                      + ((int32_t)(rframe.data[2]) <<  8)
                      + ((int32_t)(rframe.data[3]));
        double position = ((double)(count)/encorder_counts_)*M_PI;
        return position;
    }
    else
    {
        perror("读取大腿电机输出端位置异常");
        return NAN;
    }
}

double leg_motor_socketcan::leg_motor_effort_read()
{
    struct can_frame rframe;
    struct can_frame sframe;
    sframe.can_dlc = 2;
    sframe.can_id = leg_motor_id_;
    sframe.data[0] = 0x02U;
    sframe.data[1] = 0x0DU;
    if (write(socket_fd_, &sframe, sizeof(struct can_frame)) < 0) {
        perror("Error writing to socket");
        return NAN;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return NAN;
    }
    if (rframe.can_dlc != 1)
    {
        int32_t effort = ((int32_t)(rframe.data[0]) << 24)
                       + ((int32_t)(rframe.data[1]) << 16)
                       + ((int32_t)(rframe.data[2]) <<  8)
                       + ((int32_t)(rframe.data[3]));
        double outputeffort = (double)effort/1000;
        return outputeffort;
    }
    else
    {
        perror("大腿电机实际扭矩读取异常");
        return NAN;
    }
}
}